#include <math.h>

#include "ubt_com/ubt_com_util.h"
#include "rclcpp/rclcpp.hpp"


namespace ubt_com
{

double getQuaternionYaw(const tf2::Quaternion& quat) {
    // 将四元数转换为欧拉角
    tf2::Matrix3x3 mat(quat);
    double roll, pitch, yaw;
    mat.getRPY(roll, pitch, yaw);

    // 返回偏航角（yaw）
    return yaw;
}

geometry_msgs::msg::Quaternion convertThetaToQuaternion(double theta) {
    // 将欧拉角（yaw）转换为四元数
    tf2::Quaternion quat;
    quat.setRPY(0.0, 0.0, theta); // roll, pitch, yaw = 0, 0, theta

    // 将 tf2::Quaternion 转换为 geometry_msgs::msg::Quaternion
    geometry_msgs::msg::Quaternion quaternion;
    quaternion.w = quat.w();
    quaternion.x = quat.x();
    quaternion.y = quat.y();
    quaternion.z = quat.z();

    return quaternion;
}
std::string GetVersion()
{
    return "3.6.2";
}
//获取当前时间
long GetNowInMsecs()
{
    rclcpp::Time tm = rclcpp::Clock().now();
    return (long)tm.seconds() * 1000 + tm.nanoseconds() / 1e6;

}

std::vector<std::string> SpitStr(std::string strOrig, char spitChar)
{
    std::vector<std::string>  strVec;
    char *pBegin = (char *)strOrig.data(), *pos = pBegin;
    while (*pos)
    {
        if (*pos == spitChar)
        {
            if (pBegin < pos)
            {
                std::string subStr(pBegin, pos - pBegin);
                strVec.push_back(subStr);
            }
            pBegin = pos + 1;
        }
        pos++;
    }
    if (pBegin < pos)
    {
        std::string subStr(pBegin, pos - pBegin);
        strVec.push_back(subStr);
    }
    return strVec;
}

std::map<std::string, std::string> ParseUrlQuery(std::string urlQuery)
{
    std::map<std::string, std::string> paramMap;
    std::vector<std::string> paramVec = SpitStr(urlQuery, '&');
    for (std::vector<std::string>::iterator iter = paramVec.begin(); iter != paramVec.end(); ++iter)
    {
        std::vector<std::string> keyValuePair = SpitStr(*iter, '=');
        if ((keyValuePair.size() == 0) ||
                (keyValuePair.size() > 2))
        {
            continue;
        }
        std::string key, value;
        key = keyValuePair[0];
        if (keyValuePair.size() > 1)
        {
            value = keyValuePair[1];
        }
        paramMap[key] = value;
    }
    return paramMap;
}

bool isZero(double val)
{
    if ((-0.000001 < val) && (val < 0.000001))
    {
        return true;
    }
    return false;
}

double rad2Degree(double rad)
{
    return rad * 180 / M_PI;
}

double getRegularTheta(double theta)
{
    if (theta > M_PI)
    {
        theta -= 2 * M_PI;
    }
    else if (theta <= (-1 * M_PI))
    {
        theta += 2 * M_PI;
    }
    return theta;
}

/*
	从posSrc指向到posDst的theta角(弧度)
*/
double getThetaS2D(geometry_msgs::msg::Pose2D& posSrc, geometry_msgs::msg::Pose2D& posDst)
{
    double theta;
    double yOff = posDst.y - posSrc.y;
    double xOff = posDst.x - posSrc.x;
    theta = atan2(yOff, xOff);
    return theta;
}


/*
	 点1在坐标系A中为p1PoseInA,在坐标系B中为p1PoseInB;求点2(在坐标系A中为p2PoseInA)在坐标系B中的坐标
*/
geometry_msgs::msg::Pose2D getP2PoseInB(geometry_msgs::msg::Pose2D p1PoseInA, geometry_msgs::msg::Pose2D p1PoseInB, geometry_msgs::msg::Pose2D p2PoseInA)
{
    double thetaOff, xOff, yOff;
    double cosTheta, sinTheta;
    geometry_msgs::msg::Pose2D p2PoseInB;
    thetaOff = p1PoseInB.theta - p1PoseInA.theta;
    cosTheta = cos(thetaOff);
    sinTheta = sin(thetaOff);
    xOff = p1PoseInB.x - (p1PoseInA.x * cosTheta - p1PoseInA.y * sinTheta);
    yOff = p1PoseInB.y - (p1PoseInA.x * sinTheta + p1PoseInA.y * cosTheta);
    p2PoseInB.x = xOff + p2PoseInA.x * cosTheta - p2PoseInA.y * sinTheta;
    p2PoseInB.y = yOff + p2PoseInA.x * sinTheta + p2PoseInA.y * cosTheta;
    p2PoseInB.theta = getRegularTheta(thetaOff + p2PoseInA.theta);
    return p2PoseInB;
}

/*
	poseLoc,poseBase为两个点在同一坐标系下的坐标,求当以poseBase为坐标系时poseLoc的相对坐标
*/
geometry_msgs::msg::Pose2D getL2BPose(geometry_msgs::msg::Pose2D poseLoc, geometry_msgs::msg::Pose2D poseBase)
{
    geometry_msgs::msg::Pose2D poseZero, l2bPose;
    poseZero.x = poseZero.y = poseZero.theta = 0;
    l2bPose = getP2PoseInB(poseBase, poseZero, poseLoc);
    return l2bPose;
}

/*使theta从thetaSrc 切换到thetaDst所需的旋转速度,>0 逆时针,0 已到位,不用旋转,<0 顺时针*/
double getAnglularSpeed(double thetaSrc, double thetaDst, double thetaOffMax)
{
    double angular;
    double thetaDiff = getRegularTheta(thetaDst - thetaSrc);
    double thetaDiffAbs = fabs(thetaDiff);

    if (thetaDiffAbs < thetaOffMax)
    {
        angular = 0.0;
    }
    else
    {
        if (thetaDiffAbs < 0.02)
        {
            angular = 0.01;
        }
        else if (thetaDiffAbs < 0.5)
        {
            angular = 0.1;
        }
        else
        {
            angular = 0.2;
        }
        if (thetaDiff < 0)
        {
            angular *= -1;
        }
    }
    return angular;
}

}

